/****************************************************************************
 *
 *   Copyright (c) 2025 H743 Flight Control Project. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file test_px4_sem.c
 * 
 * Unit tests for PX4 semaphore wrapper
 */

#include "px4_platform_common/px4_sem.h"
#include <stdio.h>
#include <string.h>

/* External debug printf function */
extern void debug_printf(const char *format, ...);

/* Test result counters */
static int tests_passed = 0;
static int tests_failed = 0;

/* Test macros */
#define TEST_ASSERT(condition, msg) \
    do { \
        if (condition) { \
            tests_passed++; \
            debug_printf("[PASS] %s\r\n", msg); \
        } else { \
            tests_failed++; \
            debug_printf("[FAIL] %s\r\n", msg); \
        } \
    } while(0)

/**
 * @brief Test semaphore initialization and destruction
 */
static void test_sem_init_destroy(void)
{
    debug_printf("\r\n=== Test: Semaphore Init/Destroy ===\r\n");
    
    px4_sem_t sem;
    int ret;
    
    /* Test initialization with value 0 */
    ret = px4_sem_init(&sem, 0, 0);
    TEST_ASSERT(ret == 0, "px4_sem_init with value 0");
    TEST_ASSERT(sem.sem != NULL, "Semaphore handle is not NULL");
    
    /* Test getvalue */
    int val;
    ret = px4_sem_getvalue(&sem, &val);
    TEST_ASSERT(ret == 0, "px4_sem_getvalue returns success");
    TEST_ASSERT(val == 0, "Initial semaphore value is 0");
    
    /* Test destruction */
    ret = px4_sem_destroy(&sem);
    TEST_ASSERT(ret == 0, "px4_sem_destroy");
    TEST_ASSERT(sem.sem == NULL, "Semaphore handle is NULL after destroy");
    
    /* Test initialization with value 5 */
    ret = px4_sem_init(&sem, 0, 5);
    TEST_ASSERT(ret == 0, "px4_sem_init with value 5");
    
    ret = px4_sem_getvalue(&sem, &val);
    TEST_ASSERT(ret == 0 && val == 5, "Initial semaphore value is 5");
    
    px4_sem_destroy(&sem);
}

/**
 * @brief Test semaphore post and wait
 */
static void test_sem_post_wait(void)
{
    debug_printf("\r\n=== Test: Semaphore Post/Wait ===\r\n");
    
    px4_sem_t sem;
    int ret;
    
    /* Initialize with value 0 */
    px4_sem_init(&sem, 0, 0);
    
    /* Test trywait on empty semaphore (should fail) */
    ret = px4_sem_trywait(&sem);
    TEST_ASSERT(ret == -1, "px4_sem_trywait fails on empty semaphore");
    
    /* Post to semaphore */
    ret = px4_sem_post(&sem);
    TEST_ASSERT(ret == 0, "px4_sem_post");
    
    /* Check value */
    int val;
    px4_sem_getvalue(&sem, &val);
    TEST_ASSERT(val == 1, "Semaphore value is 1 after post");
    
    /* Trywait should succeed now */
    ret = px4_sem_trywait(&sem);
    TEST_ASSERT(ret == 0, "px4_sem_trywait succeeds after post");
    
    /* Check value is back to 0 */
    px4_sem_getvalue(&sem, &val);
    TEST_ASSERT(val == 0, "Semaphore value is 0 after trywait");
    
    px4_sem_destroy(&sem);
}

/**
 * @brief Test semaphore timeout
 */
static void test_sem_timedwait(void)
{
    debug_printf("\r\n=== Test: Semaphore Timedwait ===\r\n");
    
    px4_sem_t sem;
    int ret;
    
    /* Initialize with value 0 */
    px4_sem_init(&sem, 0, 0);
    
    /* Test timedwait with short timeout (should timeout) */
    uint32_t start = xTaskGetTickCount();
    ret = px4_sem_timedwait(&sem, 10);  /* 10ms timeout */
    uint32_t elapsed = xTaskGetTickCount() - start;
    
    TEST_ASSERT(ret == -1, "px4_sem_timedwait times out");
    TEST_ASSERT(elapsed >= pdMS_TO_TICKS(10), "Timeout duration is correct");
    
    /* Post and test timedwait (should succeed immediately) */
    px4_sem_post(&sem);
    start = xTaskGetTickCount();
    ret = px4_sem_timedwait(&sem, 100);
    elapsed = xTaskGetTickCount() - start;
    
    TEST_ASSERT(ret == 0, "px4_sem_timedwait succeeds when semaphore available");
    TEST_ASSERT(elapsed < pdMS_TO_TICKS(10), "Returns immediately when available");
    
    px4_sem_destroy(&sem);
}

/**
 * @brief Run all semaphore tests
 */
void px4_sem_run_tests(void)
{
    debug_printf("\r\n");
    debug_printf("========================================\r\n");
    debug_printf("  PX4 Semaphore Unit Tests\r\n");
    debug_printf("========================================\r\n");
    
    tests_passed = 0;
    tests_failed = 0;
    
    test_sem_init_destroy();
    test_sem_post_wait();
    test_sem_timedwait();
    
    debug_printf("\r\n========================================\r\n");
    debug_printf("  Test Results\r\n");
    debug_printf("========================================\r\n");
    debug_printf("  Passed: %d\r\n", tests_passed);
    debug_printf("  Failed: %d\r\n", tests_failed);
    debug_printf("  Total:  %d\r\n", tests_passed + tests_failed);
    debug_printf("========================================\r\n\r\n");
}

